#include "MLSSmoothingUpsampling.h"
#include "dialogs/MLSDialog.h"

#include <cc2sm.h>
#include <filtering.h>
#include <my_point_types.h>
#include <sm2cc.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>

MLSSmoothingUpsampling::MLSSmoothingUpsampling()
	: BaseFilter(FilterDescription("MLS smoothing", "Smooth using MLS, optionally upsample", "Smooth the cloud using Moving Least Sqares algorithm, estimate normals and optionally upsample", ":/toolbar/PclUtils/icons/mls_smoothing.png", true))
	, m_dialog(0)
	, use_poly_fit(false)
	, sqr_gauss(0.1)
	, search_radius(1.0)
{
}

MLSSmoothingUpsampling::~MLSSmoothingUpsampling()
{
	if (m_dialog)
		delete m_dialog;
}

int MLSSmoothingUpsampling::compute()
{
	
	ccPointCloud* cc_cloud = getSelectedEntityAsCCPointCloud();
	if(!cc_cloud)
		return -1;

	sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

	cc2smReader converter;
	converter.setInputCloud(cc_cloud);
	converter.getAsSM(*sm_cloud);

	pcl::fromROSMsg(*sm_cloud,*cloud);

	pcl::PointCloud<pcl::PointNormal> mls_points;

	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
	pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointNormal> mls;

	mls.setComputeNormals (true);
	mls.setInputCloud (cloud);
	mls.setPolynomialFit (use_poly_fit);
	mls.setSearchMethod (tree);
	mls.setSearchRadius (search_radius);
	

	mls.process (mls_points);

	sensor_msgs::PointCloud2::Ptr out_sm_cloud (new sensor_msgs::PointCloud2);
	pcl::toROSMsg(mls_points,*out_sm_cloud);

	sm2ccReader reader;
	ccPointCloud* out_c = new ccPointCloud;
	reader.setInputCloud(out_sm_cloud);
	int good = reader.getAsCC(out_c);
	if(good!=1){
		delete out_c;
		return -1;
	}

	ccHObject* cloud_container = new ccHObject();

	cloud_container->addChild(out_c);
	cloud_container->setName(qPrintable("mls"));

	emit newEntity(cloud_container);

	return 1;
}

int MLSSmoothingUpsampling::openDialog()
{
	if (!m_dialog)
		m_dialog = new MLSDialog();
		
	return m_dialog->exec() ? 1 : 0;
}

void MLSSmoothingUpsampling::getParametersFromDialog()
{
	assert(m_dialog);
	if (!m_dialog)
		return;

	search_radius = m_dialog->doubleSpinBox_searchRadius->value();
	use_poly_fit = m_dialog->checkBox_usePolyFit->checkState();

}